#include "ros/ros.h"
#include "serial/serial.h"
#include <string>
#include <sstream>
#include <iostream>
#include <fstream>
#include <vector>
#include <ctime>
#include <std_msgs/String.h>
#include <std_msgs/Empty.h>
#include <std_msgs/Float64.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/Vector3.h>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <ros/package.h>
#include "px4_gcs/GCSMessage.h"


using namespace std;

#define REFRESH_RATE 30


serial::Serial serialport;




int main(int argc, char **argv) {
    ros::init(argc, argv, "serial_test");
    ros::NodeHandle nodeHandle;

    std::string dev_name = "/dev/ttyUSB0";
    int baudrate = 57600;
    ros::Rate loop_rate(REFRESH_RATE);

    
    try
    {
        serialport.setPort(dev_name);
        // serialport.setPort("/dev/serial/by-id/usb-STMicroelectronics_STM32_Virtual_COM_Port_6D87155F5056-if00");
        serialport.setBaudrate(baudrate);
        serial::Timeout to = serial::Timeout::simpleTimeout(1000);
        serialport.setTimeout(to);
        serialport.open();
    }
    catch (serial::IOException &e)
    {
        ROS_ERROR_STREAM("Unable to open port ");
    }
    if (serialport.isOpen())
    {
        ROS_INFO_STREAM("Serial Port opened");
    } else
    {
        ROS_ERROR_STREAM("No port opened");
    }

    GCSMessageManager gcsMessageManager(serialport);
    gcsMessageManager.showOutput = false;

//    ros::Subscriber cam_pos_sub = nodeHandle.subscribe<geometry_msgs::PoseStamped>("/cam/pose", 1, camposcallback);
//    ros::Subscriber vicon_pos_sub = nodeHandle.subscribe<geometry_msgs::PoseStamped>("/mocap/pose", 1, viconposcallback);
    ros::Publisher cam_reset_origin_pub = nodeHandle.advertise<geometry_msgs::PoseStamped> ("/cam_reset_origin/pose", 1);
    ros::Publisher vicon_reset_origin_pub = nodeHandle.advertise<geometry_msgs::PoseStamped> ("/vicon_reset_origin/pose", 1);
    ros::Publisher pos_setpoint_pub = nodeHandle.advertise<geometry_msgs::PoseStamped> ("/pos_setpoint/pose", 1);


    ros::spinOnce();

    while (ros::ok())
    {
        CMDMessage cmd(VICON);
        cmd.backInfo = POSITION_AND_IMU;
        gcsMessageManager.sendMsg(cmd);
        CSTMessage cstMessage(SEND_NED_POSITION_RPY);
        cstMessage.position = {1,2,3};
        cstMessage.rpy = {0.1,-0.2,0.3};
        gcsMessageManager.sendMsg(cstMessage);
        auto msg = gcsMessageManager.receiveOneMsg();
        cout << msg.position[0] << " " << msg.position[1] << " " << msg.position[2] << " "
                << msg.acceleration[0] << " " << msg.acceleration[1] << " " << msg.acceleration[2] << "   ###   ";
        SPMessage spMessage(ATTITUDE_SP);
        spMessage.rpyt = {-4,-5,-6,0.5};
        gcsMessageManager.sendMsg(spMessage);
        msg = gcsMessageManager.receiveOneMsg();
        cout << msg.position[0] << " " << msg.position[1] << " " << msg.position[2] << " "
             << msg.acceleration[0] << " " << msg.acceleration[1] << " " << msg.acceleration[2] << endl;

        loop_rate.sleep();
        ros::spinOnce();
    }

    return 0;
}
